#include <string.h>
#include <sys/ioctl.h>

#include <hwicodec_common.h>
#ifdef DMALLOC
#include <imp/dmalloc.h>
#endif
static unsigned int *vtab_p = NULL;
static unsigned int *vtab_s = NULL;
static unsigned int *ptab_p = NULL;
static unsigned int *ptab_s = NULL;
static unsigned int tlba = 0;
static int private_tlb_init = 0;
static int offset = 0;
static int last_p_index = 0;
static unsigned int g_p_index[10] = {0};

int private_tlb_open(hwicodec_t *h, int ch, unsigned int tlb_vbase, unsigned int tlb_pbase)
{
	if(private_tlb_init == 1) {
		hwicodec_log(h, C_LOG_INFO, "private tlb is already open\n");
		return 0;
	}

	if(tlb_vbase == 0 || tlb_pbase == 0) {
		hwicodec_log(h, C_LOG_ERROR, "private_tlb_open tlb base error\n");
		return -1;
	}

	vtab_p = (unsigned int *)tlb_vbase;
	vtab_s = (unsigned int *)(tlb_vbase +  0x1000);
	ptab_p = (unsigned int *)tlb_pbase;
	ptab_s = (unsigned int *)(tlb_pbase +  0x1000);
	tlba  = tlb_pbase;

	if(h->soc.fd > 0) {
		struct init_buf_info ibinfo;

		ibinfo.clist = h->soc.cnpool.chn[ch].cnode.clist;
		ibinfo.buf_info.alloc_paddr = tlba;
		ibinfo.buf_info.tlb_needed = true;

		if(ioctl(h->soc.fd, IOCTL_CHANNEL_PRIVATE_TLB, &ibinfo) < 0) {
			hwicodec_log(h, C_LOG_ERROR, "ioctl private tlb error\n");
			return -1;
		}
	}
	private_tlb_init = 1;
	return 0;
}

int private_tlb_close(hwicodec_t *h, int ch)
{
	if(private_tlb_init == 0) {
		hwicodec_log(h, C_LOG_INFO, "private tlb is already close\n");
		return 0;
	}

	if(h->soc.fd > 0) {
		struct init_buf_info ibinfo;

		ibinfo.clist = h->soc.cnpool.chn[ch].cnode.clist;
		ibinfo.buf_info.alloc_paddr = tlba;
		ibinfo.buf_info.tlb_needed = false;

		if(ioctl(h->soc.fd, IOCTL_CHANNEL_PRIVATE_TLB, &ibinfo) < 0) {
			hwicodec_log(h, C_LOG_ERROR, "ioctl private tlb error\n");
			return -1;
		}
	}

	private_tlb_init = 0;
	return 0;
}

int private_tlb_map(unsigned int v_addr, unsigned int p_addr, int length, int pad)
{
	if (private_tlb_init == 0) {
		hwicodec_log(NULL, C_LOG_ERROR, "%s: private tlb is not open\n", __func__);
		return -1;
	}

	int i;
	unsigned int fvaddr = v_addr;
	unsigned int fpaddr = p_addr;
	unsigned int lpaddr = v_addr + length - 1;
	unsigned int p_indx, s_indx;
	unsigned int paddr;
	unsigned int stlba;
	unsigned int vaddr_high_20bits;

	if(pad == 0) {
		offset = 0;
		last_p_index = 0;
		memset(g_p_index, 0, sizeof(g_p_index));
	}

	int num = (length + 4095) / 4096;
	if(((fpaddr & 0xFFFFF000) + num*4096) < lpaddr)
		num = num + 1;

	for(i = 0; i < num; i++){
		paddr = fpaddr + i*4096;
		vaddr_high_20bits = ((fvaddr>>12) & 0xFFFFF) + i;
		p_indx = (vaddr_high_20bits>>10) & 0x3FF;
		s_indx = (vaddr_high_20bits) & 0x3FF;
		if(p_indx != last_p_index) {
			int j = 0;
			for(j = 0; j < 10; j++) {
				if(g_p_index[j] == p_indx) {
					offset = j;
					break;
				} else if(g_p_index[j] == 0) {
					offset = j;
					g_p_index[j] = p_indx;
					break;
				}
			}

			if(j == 10) {
				hwicodec_log(NULL, C_LOG_ERROR, "tlb error\n");
				return -1;
			}

			last_p_index = p_indx;
		}
		stlba = (int)(ptab_s + offset * 1024);
		vtab_p[p_indx] = (stlba & 0xFFFFF000) | 0x1;
		*(vtab_s + offset * 1024 + s_indx) = (paddr & 0xFFFFF000) | (0 & 0xFFF);
	}

	return 0;
}